/*
* Copyright (c) 2017, STMicroelectronics - All Rights Reserved
*
* This file is part of VL53L1 Core and is dual licensed,
* either 'STMicroelectronics
* Proprietary license'
* or 'BSD 3-clause "New" or "Revised" License' , at your option.
*
********************************************************************************
*
* 'STMicroelectronics Proprietary license'
*
********************************************************************************
*
* License terms: STMicroelectronics Proprietary in accordance with licensing
* terms at www.st.com/sla0081
*
* STMicroelectronics confidential
* Reproduction and Communication of this document is strictly prohibited unless
* specifically authorized in writing by STMicroelectronics.
*
*
********************************************************************************
*
* Alternatively, VL53L1 Core may be distributed under the terms of
* 'BSD 3-clause "New" or "Revised" License', in which case the following
* provisions apply instead of the ones mentioned above :
*
********************************************************************************
*
* License terms: BSD 3-clause "New" or "Revised" License.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*
********************************************************************************
*
*/

/**
 * @file  vl53l1_api_core.c
 *
 * @brief EwokPlus25 low level API function definition
 */


#include "vl53l1_ll_def.h"
#include "vl53l1_ll_device.h"
#include "vl53l1_platform.h"
#include "vl53l1_register_map.h"
#include "vl53l1_register_funcs.h"
#include "vl53l1_register_settings.h"
#include "vl53l1_core.h"
#include "vl53l1_wait.h"
#include "vl53l1_api_preset_modes.h"
#include "vl53l1_silicon_core.h"
#include "vl53l1_api_core.h"
#include "vl53l1_api_calibration.h"

#ifdef VL53L1_LOG_ENABLE
#include "vl53l1_api_debug.h"
#endif
#ifdef VL53L1_LOGGING
#include "vl53l1_debug.h"
#endif

#define LOG_FUNCTION_START(fmt, ...) \
    _LOG_FUNCTION_START(VL53L1_TRACE_MODULE_CORE, fmt, ##__VA_ARGS__)
#define LOG_FUNCTION_END(status, ...) \
    _LOG_FUNCTION_END(VL53L1_TRACE_MODULE_CORE, status, ##__VA_ARGS__)
#define LOG_FUNCTION_END_FMT(status, fmt, ...)              \
    _LOG_FUNCTION_END_FMT(VL53L1_TRACE_MODULE_CORE, status, \
                          fmt, ##__VA_ARGS__)

#define trace_print(level, ...)                \
    _LOG_TRACE_PRINT(VL53L1_TRACE_MODULE_CORE, \
                     level, VL53L1_TRACE_FUNCTION_NONE, ##__VA_ARGS__)


#ifndef VL53L1_NOCALIB
VL53L1_Error VL53L1_run_ref_spad_char(
    VL53L1_DEV    Dev,
    VL53L1_Error *pcal_status)
{
    /*
	 *  Runs Reference SPAD Characterisation
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);

    uint8_t comms_buffer[6];

    VL53L1_refspadchar_config_t *prefspadchar = &(pdev->refspadchar);

    LOG_FUNCTION_START("");

    /*
	 * Ensure power force is enabled
	 */

    if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
        status = VL53L1_enable_powerforce(Dev);

    /*
	 * Configure device
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_set_ref_spad_char_config(
                Dev,
                prefspadchar->vcsel_period,
                prefspadchar->timeout_us,
                prefspadchar->target_count_rate_mcps,
                prefspadchar->max_count_rate_limit_mcps,
                prefspadchar->min_count_rate_limit_mcps,
                pdev->stat_nvm.osc_measured__fast_osc__frequency);

    /*
	 * Run device test
	 */

    if (status == VL53L1_ERROR_NONE)
        status = VL53L1_run_device_test(
            Dev,
            prefspadchar->device_test_mode);

    /*
	 * Read results
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_ReadMulti(
                Dev,
                VL53L1_REF_SPAD_CHAR_RESULT__NUM_ACTUAL_REF_SPADS,
                comms_buffer,
                2);

    if (status == VL53L1_ERROR_NONE)
    {
        pdev->dbg_results.ref_spad_char_result__num_actual_ref_spads =
            comms_buffer[0];
        pdev->dbg_results.ref_spad_char_result__ref_location =
            comms_buffer[1];
    }

    /*
	 * copy results to customer nvm managed G02 registers
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WriteMulti(
                Dev,
                VL53L1_REF_SPAD_MAN__NUM_REQUESTED_REF_SPADS,
                comms_buffer,
                2);

    if (status == VL53L1_ERROR_NONE)
    {
        pdev->customer.ref_spad_man__num_requested_ref_spads =
            comms_buffer[0];
        pdev->customer.ref_spad_man__ref_location =
            comms_buffer[1];
    }

    /* After Ref Spad Char the final set of good SPAD enables
	 * are stored in the NCY results registers below
	 *
	 *  - RESULT__SPARE_0_SD_1
	 *  - RESULT__SPARE_1_SD_1
	 *  - RESULT__SPARE_2_SD_1
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_ReadMulti(
                Dev,
                VL53L1_RESULT__SPARE_0_SD1,
                comms_buffer,
                6);

    /*
	 * copy reference SPAD enables to customer nvm managed
	 * G02 registers
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_WriteMulti(
                Dev,
                VL53L1_GLOBAL_CONFIG__SPAD_ENABLES_REF_0,
                comms_buffer,
                6);

    if (status == VL53L1_ERROR_NONE)
    {
        pdev->customer.global_config__spad_enables_ref_0 =
            comms_buffer[0];
        pdev->customer.global_config__spad_enables_ref_1 =
            comms_buffer[1];
        pdev->customer.global_config__spad_enables_ref_2 =
            comms_buffer[2];
        pdev->customer.global_config__spad_enables_ref_3 =
            comms_buffer[3];
        pdev->customer.global_config__spad_enables_ref_4 =
            comms_buffer[4];
        pdev->customer.global_config__spad_enables_ref_5 =
            comms_buffer[5];
    }

#ifdef VL53L1_LOG_ENABLE
    /* Print customer nvm managed data */
    if (status == VL53L1_ERROR_NONE)
        VL53L1_print_customer_nvm_managed(
            &(pdev->customer),
            "run_ref_spad_char():pdev->lldata.customer.",
            VL53L1_TRACE_MODULE_REF_SPAD_CHAR);
#endif

    if (status == VL53L1_ERROR_NONE)
    {
        switch (pdev->sys_results.result__range_status)
        {
        case VL53L1_DEVICEERROR_REFSPADCHARNOTENOUGHDPADS:
            status = VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS;
            break;

        case VL53L1_DEVICEERROR_REFSPADCHARMORETHANTARGET:
            status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH;
            break;

        case VL53L1_DEVICEERROR_REFSPADCHARLESSTHANTARGET:
            status = VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW;
            break;
        }
    }

    /*
	 * Save unfiltered status
	 */

    *pcal_status = status;

    /* Status exception code */

    IGNORE_STATUS(
        IGNORE_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
        VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS,
        status);

    IGNORE_STATUS(
        IGNORE_REF_SPAD_CHAR_RATE_TOO_HIGH,
        VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH,
        status);

    IGNORE_STATUS(
        IGNORE_REF_SPAD_CHAR_RATE_TOO_LOW,
        VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW,
        status);


    LOG_FUNCTION_END(status);

    return status;
}

VL53L1_Error VL53L1_run_offset_calibration(
    VL53L1_DEV    Dev,
    int16_t       cal_distance_mm,
    VL53L1_Error *pcal_status)
{
    /*
	 * Runs offset calibration
	 *
	 * Recommended tuning parm settings:
	 *
	 *  - pre_num_of_samples    =    32
	 *  - mm1_num_of_samples    =   100
	 *  - mm2_num_of_samples    =    64
	 *  - target_distance_mm    =   140mm
	 *  - target reflectance    =     5%
	 *
	 * Standard Ranging (sigma delta mode):
	 *  - dss_config__target_total_rate_mcps = 20.0 -40.0 Mcps
	 *  - phasecal_config_timeout_us        =  1000
	 *  - range_config_timeout_us           = 13000
	 *  - mm_config_timeout_us              = 13000
	 *
	 *
	 * Note: function parms simplified as part of
	 * Patch_CalFunctionSimplification_11791
	 *
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev =
        VL53L1DevStructGetLLDriverHandle(Dev);

    VL53L1_DevicePresetModes device_preset_modes[VL53L1_MAX_OFFSET_RANGE_RESULTS];

    VL53L1_range_results_t      range_results;
    VL53L1_range_results_t     *prange_results = &range_results;
    VL53L1_range_data_t        *prange_data    = NULL;
    VL53L1_offset_range_data_t *poffset        = NULL;

    uint8_t i = 0;
    uint8_t m = 0;
    uint8_t measurement_mode =
        VL53L1_DEVICEMEASUREMENTMODE_BACKTOBACK;
    uint16_t manual_effective_spads =
        pdev->gen_cfg.dss_config__manual_effective_spads_select;

    uint8_t num_of_samples[VL53L1_MAX_OFFSET_RANGE_RESULTS];

    LOG_FUNCTION_START("");

    /* select requested offset calibration mode */

    switch (pdev->offset_calibration_mode)
    {
    default:
        device_preset_modes[0] =
            VL53L1_DEVICEPRESETMODE_STANDARD_RANGING;
        device_preset_modes[1] =
            VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM1_CAL;
        device_preset_modes[2] =
            VL53L1_DEVICEPRESETMODE_STANDARD_RANGING_MM2_CAL;
        break;
    }

    /* initialise num_of_samples */
    /* Start Patch_CalFunctionSimplification_11791 */
    num_of_samples[0] = pdev->offsetcal_cfg.pre_num_of_samples;
    num_of_samples[1] = pdev->offsetcal_cfg.mm1_num_of_samples;
    num_of_samples[2] = pdev->offsetcal_cfg.mm2_num_of_samples;
    /* End Patch_CalFunctionSimplification_11791 */

    /* force all offsets to zero */

    switch (pdev->offset_calibration_mode)
    {
    case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:
        /* only run pre range */
        pdev->offset_results.active_results = 1;

        break;

    default:

        pdev->customer.mm_config__inner_offset_mm = 0;
        pdev->customer.mm_config__outer_offset_mm = 0;
        pdev->offset_results.active_results =
            VL53L1_MAX_OFFSET_RANGE_RESULTS;

        break;
    }

    pdev->customer.algo__part_to_part_range_offset_mm = 0;

    /* initialise offset range results */

    pdev->offset_results.max_results     = VL53L1_MAX_OFFSET_RANGE_RESULTS;
    pdev->offset_results.cal_distance_mm = cal_distance_mm;

    for (m = 0; m < VL53L1_MAX_OFFSET_RANGE_RESULTS; m++)
    {
        poffset                  = &(pdev->offset_results.data[m]);
        poffset->preset_mode     = 0;
        poffset->no_of_samples   = 0;
        poffset->effective_spads = 0;
        poffset->peak_rate_mcps  = 0;
        poffset->sigma_mm        = 0;
        poffset->median_range_mm = 0;
    }

    for (m = 0; m < pdev->offset_results.active_results; m++)
    {
        poffset = &(pdev->offset_results.data[m]);

        poffset->preset_mode = device_preset_modes[m];

        /* Apply preset mode */

        if (status == VL53L1_ERROR_NONE)
            status =
                VL53L1_set_preset_mode(
                    Dev,
                    device_preset_modes[m],
                    /* Start Patch_CalFunctionSimplification_11791 */
                    pdev->offsetcal_cfg.dss_config__target_total_rate_mcps,
                    pdev->offsetcal_cfg.phasecal_config_timeout_us,
                    pdev->offsetcal_cfg.mm_config_timeout_us,
                    pdev->offsetcal_cfg.range_config_timeout_us,
                    /* End Patch_CalFunctionSimplification_11791 */
                    100);

        pdev->gen_cfg.dss_config__manual_effective_spads_select =
            manual_effective_spads;

        /* Initialise device and start range */

        if (status == VL53L1_ERROR_NONE)
            status =
                VL53L1_init_and_start_range(
                    Dev,
                    measurement_mode,
                    VL53L1_DEVICECONFIGLEVEL_CUSTOMER_ONWARDS);

        for (i = 0; i <= (num_of_samples[m] + 2); i++)
        {
            /* Wait for range completion */

            if (status == VL53L1_ERROR_NONE)
                status =
                    VL53L1_wait_for_range_completion(Dev);

            /*
			 *  Get Device Results
			 *  - Checks the stream count is the expected one
			 *  - Read device system results
			 */

            if (status == VL53L1_ERROR_NONE)
                status =
                    VL53L1_get_device_results(
                        Dev,
                        VL53L1_DEVICERESULTSLEVEL_FULL,
                        prange_results);

            /*
			 * Ignore 1st two ranges to give the sigma delta initial
			 * phase time to settle
			 *
			 * accummulate range results if range is successful
			 */

            prange_data = &(prange_results->data[0]);

            if (prange_results->stream_count > 1)
            {
                if (prange_data->range_status == VL53L1_DEVICEERROR_RANGECOMPLETE)
                {
                    poffset->no_of_samples++;
                    poffset->effective_spads +=
                        (uint32_t)prange_data->actual_effective_spads;
                    poffset->peak_rate_mcps +=
                        (uint32_t)prange_data->peak_signal_count_rate_mcps;
                    poffset->sigma_mm +=
                        (uint32_t)prange_data->sigma_mm;
                    poffset->median_range_mm +=
                        (int32_t)prange_data->median_range_mm;

                    poffset->dss_config__roi_mode_control =
                        pdev->gen_cfg.dss_config__roi_mode_control;
                    poffset->dss_config__manual_effective_spads_select =
                        pdev->gen_cfg.dss_config__manual_effective_spads_select;
                }
            }

            /*
			 * Conditional wait for firmware ready. Only waits for timed
			 * and single shot modes. Mode check is performed inside the
			 * wait function
			 */

            if (status == VL53L1_ERROR_NONE)
                status =
                    VL53L1_wait_for_firmware_ready(Dev);

            /*
			 * Send ranging handshake
			 *
			 *  - Update Zone management
			 *  - Update GPH registers
			 *  - Clear current interrupt
			 *  - Initialise SYSTEM__MODE_START for next range (if there is one!)
			 */

            if (status == VL53L1_ERROR_NONE)
                status =
                    VL53L1_clear_interrupt_and_enable_next_range(
                        Dev,
                        measurement_mode);
        }

        /* Stop range */

        if (status == VL53L1_ERROR_NONE)
            status = VL53L1_stop_range(Dev);

        /* Wait for Stop (abort) range to complete */

        if (status == VL53L1_ERROR_NONE)
            status = VL53L1_WaitUs(Dev, 1000);

        /* generate average values */
        if (poffset->no_of_samples > 0)
        {
            poffset->effective_spads += (poffset->no_of_samples / 2);
            poffset->effective_spads /= poffset->no_of_samples;

            poffset->peak_rate_mcps += (poffset->no_of_samples / 2);
            poffset->peak_rate_mcps /= poffset->no_of_samples;

            poffset->sigma_mm += (poffset->no_of_samples / 2);
            poffset->sigma_mm /= poffset->no_of_samples;

            poffset->median_range_mm += (poffset->no_of_samples / 2);
            poffset->median_range_mm /= poffset->no_of_samples;

            poffset->range_mm_offset  = (int32_t)cal_distance_mm;
            poffset->range_mm_offset -= poffset->median_range_mm;

            /* remember the number of SPADs for standard ranging */
            if (poffset->preset_mode == VL53L1_DEVICEPRESETMODE_STANDARD_RANGING)
                manual_effective_spads =
                    (uint16_t)poffset->effective_spads;
        }
    }

    /* Calculate offsets */

    switch (pdev->offset_calibration_mode)
    {
    case VL53L1_OFFSETCALIBRATIONMODE__MM1_MM2__STANDARD_PRE_RANGE_ONLY:

        /* copy offsets to customer data structure */
        pdev->customer.mm_config__inner_offset_mm +=
            (int16_t)pdev->offset_results.data[0].range_mm_offset;
        pdev->customer.mm_config__outer_offset_mm +=
            (int16_t)pdev->offset_results.data[0].range_mm_offset;
        break;

    default:
        /* copy offsets to customer data structure */
        pdev->customer.mm_config__inner_offset_mm =
            (int16_t)pdev->offset_results.data[1].range_mm_offset;
        pdev->customer.mm_config__outer_offset_mm =
            (int16_t)pdev->offset_results.data[2].range_mm_offset;
        pdev->customer.algo__part_to_part_range_offset_mm = 0;

        /* copy average rate and effective SPAD count to
		   additional offset calibration data structure */

        pdev->add_off_cal_data.result__mm_inner_actual_effective_spads =
            (uint16_t)pdev->offset_results.data[1].effective_spads;
        pdev->add_off_cal_data.result__mm_outer_actual_effective_spads =
            (uint16_t)pdev->offset_results.data[2].effective_spads;

        pdev->add_off_cal_data.result__mm_inner_peak_signal_count_rtn_mcps =
            (uint16_t)pdev->offset_results.data[1].peak_rate_mcps;
        pdev->add_off_cal_data.result__mm_outer_peak_signal_count_rtn_mcps =
            (uint16_t)pdev->offset_results.data[2].peak_rate_mcps;

        break;
    }


    /* apply to device */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_set_customer_nvm_managed(
                Dev,
                &(pdev->customer));

    /*
	 *  Check the peak rates, sigma, min spads for each stage
	 */

    for (m = 0; m < pdev->offset_results.active_results; m++)
    {
        poffset = &(pdev->offset_results.data[m]);

        if (status == VL53L1_ERROR_NONE)
        {
            pdev->offset_results.cal_report = m;

            if (poffset->no_of_samples < num_of_samples[m])
                status = VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES;

            /* only check sigma for the pre-range as
			 * the it is not calculated by the device
			 * for the MM1 and MM2 stages
			 */
            if (m == 0 && poffset->sigma_mm > ((uint32_t)VL53L1_OFFSET_CAL_MAX_SIGMA_MM << 5))
                status = VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH;

            if (poffset->peak_rate_mcps > VL53L1_OFFSET_CAL_MAX_PRE_PEAK_RATE_MCPS)
                status = VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH;

            if (poffset->dss_config__manual_effective_spads_select < VL53L1_OFFSET_CAL_MIN_EFFECTIVE_SPADS)
                status = VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW;

            if (poffset->dss_config__manual_effective_spads_select == 0)
                status = VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL;

            if (poffset->no_of_samples == 0)
                status = VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL;
        }
    }

    /*
	 * Save unfiltered status
	 */

    pdev->offset_results.cal_status = status;
    *pcal_status                    = pdev->offset_results.cal_status;

    /* Status exception codes */

    IGNORE_STATUS(
        IGNORE_OFFSET_CAL_MISSING_SAMPLES,
        VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES,
        status);

    IGNORE_STATUS(
        IGNORE_OFFSET_CAL_SIGMA_TOO_HIGH,
        VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH,
        status);

    IGNORE_STATUS(
        IGNORE_OFFSET_CAL_RATE_TOO_HIGH,
        VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH,
        status);

    IGNORE_STATUS(
        IGNORE_OFFSET_CAL_SPAD_COUNT_TOO_LOW,
        VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW,
        status);

#ifdef VL53L1_LOG_ENABLE

    /* Prints out the offset calibration data for debug */

    VL53L1_print_customer_nvm_managed(
        &(pdev->customer),
        "run_offset_calibration():pdev->lldata.customer.",
        VL53L1_TRACE_MODULE_OFFSET_DATA);

    VL53L1_print_additional_offset_cal_data(
        &(pdev->add_off_cal_data),
        "run_offset_calibration():pdev->lldata.add_off_cal_data.",
        VL53L1_TRACE_MODULE_OFFSET_DATA);

    VL53L1_print_offset_range_results(
        &(pdev->offset_results),
        "run_offset_calibration():pdev->lldata.offset_results.",
        VL53L1_TRACE_MODULE_OFFSET_DATA);
#endif

    LOG_FUNCTION_END(status);

    return status;
}
#endif

#ifndef VL53L1_NOCALIB
VL53L1_Error VL53L1_run_spad_rate_map(
    VL53L1_DEV               Dev,
    VL53L1_DeviceTestMode    device_test_mode,
    VL53L1_DeviceSscArray    array_select,
    uint32_t                 ssc_config_timeout_us,
    VL53L1_spad_rate_data_t *pspad_rate_data)
{
    /**
     *  Runs SPAD Rate Map
     */

    VL53L1_Error status = VL53L1_ERROR_NONE;

    VL53L1_LLDriverData_t *pdev =
        VL53L1DevStructGetLLDriverHandle(Dev);

    LOG_FUNCTION_START("");

    /*
	 * Ensure power force is enabled
	 */
    if (status == VL53L1_ERROR_NONE)
        status = VL53L1_enable_powerforce(Dev);

    /*
	 * Configure the test
	 */

    if (status == VL53L1_ERROR_NONE)
    {
        pdev->ssc_cfg.array_select = array_select;
        pdev->ssc_cfg.timeout_us   = ssc_config_timeout_us;
        status =
            VL53L1_set_ssc_config(
                Dev,
                &(pdev->ssc_cfg),
                pdev->stat_nvm.osc_measured__fast_osc__frequency);
    }

    /*
	 * Run device test
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_run_device_test(
                Dev,
                device_test_mode);

    /*
	 * Read Rate Data from Patch Ram
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_get_spad_rate_data(
                Dev,
                pspad_rate_data);

    if (device_test_mode == VL53L1_DEVICETESTMODE_LCR_VCSEL_ON)
        pspad_rate_data->fractional_bits = 7;
    else
        pspad_rate_data->fractional_bits = 15;

    /* Ensure power force is disabled */

    if (status == VL53L1_ERROR_NONE)
        status = VL53L1_disable_powerforce(Dev);

#ifdef VL53L1_LOG_ENABLE
    /* Print return rate data and map */

    if (status == VL53L1_ERROR_NONE)
    {
        VL53L1_print_spad_rate_data(
            pspad_rate_data,
            "run_spad_rate_map():",
            VL53L1_TRACE_MODULE_SPAD_RATE_MAP);
        VL53L1_print_spad_rate_map(
            pspad_rate_data,
            "run_spad_rate_map():",
            VL53L1_TRACE_MODULE_SPAD_RATE_MAP);
    }
#endif

    LOG_FUNCTION_END(status);

    return status;
}
#endif


#ifndef VL53L1_NOCALIB
VL53L1_Error VL53L1_run_device_test(
    VL53L1_DEV            Dev,
    VL53L1_DeviceTestMode device_test_mode)
{
    /*
	 *  Runs the selected Device Test Mode
	 */

    VL53L1_Error           status = VL53L1_ERROR_NONE;
    VL53L1_LLDriverData_t *pdev   = VL53L1DevStructGetLLDriverHandle(Dev);

    uint8_t comms_buffer[2];
    uint8_t gpio_hv_mux__ctrl = 0;

    LOG_FUNCTION_START("");

    /*
	 * Get current interrupt config
	 */

    if (status == VL53L1_ERROR_NONE) /*lint !e774 always true*/
        status =
            VL53L1_RdByte(
                Dev,
                VL53L1_GPIO_HV_MUX__CTRL,
                &gpio_hv_mux__ctrl);

    if (status == VL53L1_ERROR_NONE)
        pdev->stat_cfg.gpio_hv_mux__ctrl = gpio_hv_mux__ctrl;

    /*
	 * Trigger the test
	 */
    if (status == VL53L1_ERROR_NONE)
        status = VL53L1_start_test(
            Dev,
            device_test_mode);

    /*
	 * Wait for test completion
	 */
    if (status == VL53L1_ERROR_NONE)
        status = VL53L1_wait_for_test_completion(Dev);

    /*
	 * Read range and report status
	 */
    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_ReadMulti(
                Dev,
                VL53L1_RESULT__RANGE_STATUS,
                comms_buffer,
                2);

    if (status == VL53L1_ERROR_NONE)
    {
        pdev->sys_results.result__range_status  = comms_buffer[0];
        pdev->sys_results.result__report_status = comms_buffer[1];
    }

    /* mask range status bits */

    pdev->sys_results.result__range_status &=
        VL53L1_RANGE_STATUS__RANGE_STATUS_MASK;

    if (status == VL53L1_ERROR_NONE)
    {
        trace_print(
            VL53L1_TRACE_LEVEL_INFO,
            "    Device Test Complete:\n\t%-32s = %3u\n\t%-32s = %3u\n",
            "result__range_status",
            pdev->sys_results.result__range_status,
            "result__report_status",
            pdev->sys_results.result__report_status);

        /*
		 * Clear interrupt
		 */
        if (status == VL53L1_ERROR_NONE)
            status = VL53L1_clear_interrupt(Dev);
    }

    /*
	 * Clear test mode register
	 *  - required so that next test command will trigger
	 *    internal MCU interrupt
	 */

    if (status == VL53L1_ERROR_NONE)
        status =
            VL53L1_start_test(
                Dev,
                0x00);

    LOG_FUNCTION_END(status);

    return status;
}
#endif
